import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32
from tf_transformations import quaternion_from_euler
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped

import tf_transformations
import threading
import time
import math
from rclpy.executors import MultiThreadedExecutor

import subprocess
import re
import queue
import os
import numpy as np

from agv_common_class import NavigationClient, AngleSubscriber, OdomSubscriber, RotateRobot, AngleNavGoalPoseSend

# ours
# 大模型
from spacemit_llm import LLMModel, FCModel

# 音频
from spacemit_audio import find_audio_card, find_playback_card
from spacemit_audio import play_wav_non_blocking, play_wav, play_audio_in_thread
from spacemit_audio import ASRModel
from spacemit_audio import RecAudioThreadPipeLine, RecAudioPipeLine
from spacemit_audio.record_loop import RecAudioDB
asr_model = ASRModel()
from tools.agv import FuncControllerNode, execute_command

# 导入军事目标检测模块
from military_target_detection import MilitaryTargetDetection

# 导入日志记录模块
from agv_logger import AGVLogger

record_device = find_audio_card()
play_device = f'plughw:{find_playback_card()},0'


def run_executor(executor):
    executor.spin()  # 运行 ROS 2 事件循环（不会阻塞主线程）

def main():
    rclpy.init()

    # 初始化日志记录器
    logger = AGVLogger()
    logger.log_system_event("ROS节点初始化")

    func_controller_node = FuncControllerNode() # 总控制
    angle_sub_node = AngleSubscriber()  # 声源定位信息订阅
    odom_sub_node = OdomSubscriber()    # 里程计信息订阅
    node_rotate = RotateRobot(odom_sub_node, angle_sub_node)  # 传递订阅节点的引用

    nav_pub = AngleNavGoalPoseSend(angle_sub_node, invert=True)

    # 多线程执行的管理器
    executor = MultiThreadedExecutor()
    executor.add_node(func_controller_node)
    executor.add_node(angle_sub_node)
    executor.add_node(odom_sub_node)
    executor.add_node(node_rotate)
    executor.add_node(nav_pub)

    executor_thread = threading.Thread(target=run_executor, args=(executor,), daemon=True)
    executor_thread.start()
    
    logger.log_system_event("执行器线程启动")

    # 初始化军事目标检测 - 设置为持续运行
    target_detector = MilitaryTargetDetection(
        model_path="/home/sy/jobot-ai-pipeline/models/resnet50.q.onnx",
        labels_path="/home/sy/jobot-ai-pipeline/data/labels.txt",
        play_device=play_device,
        logger=logger,  # 传递日志记录器
        continuous_mode=True  # 设置为持续运行模式
    )
    
    # 启动目标检测
    if not target_detector.start_detection():
        print("警告: 无法启动目标检测，将不会记录目标信息")
        logger.log_system_event("目标检测启动失败")
    else:
        logger.log_system_event("目标检测启动成功")
    
    # 添加目标检测命令到功能字典
    func_controller_node.function_dict_zh.update({
        "检测目标": "detect_targets",
        "识别目标": "detect_targets",
        "查看周围": "detect_targets",
        "有什么目标": "detect_targets"
    })
    
    # 目标变化检测变量
    last_detected_target = None
    target_change_threshold = 0.2  # 置信度变化阈值
    
    # 添加目标检测函数
    def detect_targets():
        """目标检测功能 - 立即报告当前检测到的目标"""
        logger.log_operation("目标检测", "立即报告当前目标")
        print("立即报告当前目标...")
        
        # 获取当前检测结果
        target, confidence = target_detector.get_latest_result()
        
        if target != "无目标":
            # 播放开始检测提示
            wav_file_path = "tools/feedback_voice/kaishijianceshibie.wav"
            play_wav(wav_file_path, device=play_device)
            
            # 播报结果
            detected_target = target_detector.speak_target()
            print(f"当前目标: {detected_target}")
        else:
            print("未检测到目标")
            play_wav("tools/feedback_voice/weifaxian_mubiao.wav", device=play_device)
    
    # 将函数添加到功能字典
    func_controller_node.function_dict["detect_targets"] = detect_targets

    # 录音功能类初始化
    rec_audio = RecAudioDB(sld=1, min_db=4000, max_time=30, rate=16000, device_index=record_device)
    logger.log_system_event("音频模块初始化完成")

    wav_file_path = "tools/feedback_voice/dengdaihuanxing.wav"
    play_wav(wav_file_path, device=play_device)
    logger.log_operation("系统启动", "播放等待唤醒提示")
    
    try:
        # 目标检测监控线程
        def target_monitor():
            """持续监控目标变化并记录"""
            nonlocal last_detected_target
            
            while rclpy.ok():
                try:
                    # 检查目标检测器是否正常运行
                    if not target_detector.is_running:
                        time.sleep(2.0)
                        continue
                    
                    # 获取最新检测结果
                    current_target, current_confidence = target_detector.get_latest_result()
                    
                    # 检查目标是否有显著变化
                    if current_target != "无目标" and (
                        last_detected_target is None or 
                        current_target != last_detected_target[0] or 
                        abs(current_confidence - last_detected_target[1]) > target_change_threshold
                    ):
                        print(f"检测到新目标: {current_target} (置信度: {current_confidence:.2f})")
                        logger.log_target_detection(current_target, current_confidence)
                        last_detected_target = (current_target, current_confidence)
                    
                    # 检查间隔
                    time.sleep(1.0)  # 每秒检查一次
                except Exception as e:
                    print(f"目标监控错误: {e}")
                    time.sleep(1.0)
        
        # 启动目标监控线程
        monitor_thread = threading.Thread(target=target_monitor, daemon=True)
        monitor_thread.start()
        logger.log_system_event("目标监控线程启动")
        
        # 主循环
        while rclpy.ok():
            print("等待唤醒...")
            logger.log_system_event("进入等待唤醒状态")
            
            # 设置等待唤醒状态
            angle_sub_node.ready_to_wait_event.set()
            angle_sub_node.trigger_event.wait()
            angle_sub_node.trigger_event.clear()
            angle_sub_node.ready_to_wait_event.clear()

            nav_pub.angle_sub = angle_sub_node.angle
            nav_pub.calc_target_point() # 计算导航的目标点
            
            func_controller_node.robot_stop_move() # 先停止小车跟随
            logger.log_operation("机器人控制", "停止移动")
            
            node_rotate.rotate_to_angle()  # 触发旋转
            logger.log_operation("机器人控制", "旋转到声源方向")

            # 播放唤醒反馈
            wav_file_path = "tools/feedback_voice/awake.wav"
            play_wav(wav_file_path, device=play_device)
            logger.log_operation("语音反馈", "播放唤醒提示")

            # 语音命令处理循环
            while rclpy.ok():
                logger.log_system_event("进入语音命令接收状态")
            
                # 开始录制用户声音
                audio_ret = rec_audio.record_audio()

                if rec_audio.exit_mode == 0:
                    # 语音转文字
                    print("开始语音转文字----------------------")
                    text = asr_model.generate(audio_ret)
                    print('user: ', text)
                    logger.log_voice_command(text)

                    # 模糊匹配
                    ret, func_name = execute_command(text, func_controller_node.function_dict_zh)
                    if ret:
                        print(f"调用的函数名: {func_name}")
                        logger.log_operation("命令执行", f"识别到命令: {func_name}")
                        
                        if func_name == 'rotate_in_place':
                            wav_file_path = "tools/feedback_voice/haode.wav"
                            play_wav_non_blocking(wav_file_path, device=play_device)
                            func_controller_node.robot_stop_move()
                            logger.log_operation("机器人控制", "原地旋转")

                        if func_name == 'follow_me':
                            wav_file_path = "tools/feedback_voice/haodekaishigensui.wav"
                            play_wav_non_blocking(wav_file_path, device=play_device)
                            logger.log_operation("机器人控制", "开始跟随")

                        if func_name == 'come_here':
                            nav_pub.publish_pose()
                            wav_file_path = "tools/feedback_voice/zhengzaiqianwang.wav"
                            play_wav(wav_file_path, device=play_device)
                            logger.log_operation("导航控制", "前往目标位置")
                        
                        # 特殊处理目标检测
                        if func_name == 'detect_targets':
                            detect_targets()
                        else:
                            # 记录操作执行
                            logger.log_operation("命令执行", f"执行函数: {func_name}")
                            func_controller_node.function_dict[func_name]()

                        if func_name == 'move_away':
                            wav_file_path = "tools/feedback_voice/zaidengzhe.wav"
                            play_wav(wav_file_path, device=play_device)
                            logger.log_operation("机器人控制", "移动到安全位置")

                        break

                    else:
                        wav_file_path = "tools/feedback_voice/meitingqingchu.wav"
                        play_wav(wav_file_path, device=play_device)
                        logger.log_operation("语音反馈", "播放未听清提示")


                else:
                    print(f"超过设定时间未检测到人声, 进入唤醒模式！")
                    wav_file_path = "tools/feedback_voice/jinruhuanxing.wav"
                    play_wav(wav_file_path, device=play_device)
                    logger.log_system_event("超时未检测到人声，返回唤醒模式")
                    break

    except KeyboardInterrupt:
        print("程序终止")
        logger.log_system_event("程序被用户中断")

    except Exception as e:
        print(f"程序发生错误: {e}")
        logger.log_system_event(f"程序发生错误: {str(e)}")

    finally:
        node_rotate.stop_move()
        logger.log_operation("机器人控制", "停止移动")
        
        # 停止目标检测
        if hasattr(target_detector, 'stop_detection'):
            target_detector.stop_detection()
            logger.log_system_event("目标检测模块已停止")
        
        time.sleep(0.1)
        executor.shutdown()
        logger.log_system_event("执行器关闭")
        
        if rclpy.ok():  # 只有在 rclpy 仍然运行时才调用 shutdown
            rclpy.shutdown()
        
        # 停止日志记录器
        logger.stop()

if __name__ == '__main__':
    # 设置环境变量强制使用V4L2
    os.environ["OPENCV_VIDEOIO_PRIORITY_V4L2"] = "1"
    os.environ["OPENCV_LOG_LEVEL"] = "ERROR"  # 减少OpenCV的日志输出
    
    main()